/* gm_server.c -- Servidor que recebe as informações da visão.
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part os GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <string.h>

#include "gm_server.h"
#include "gm_utils.h"

/* Defines */
#define NUMBER_OF_ROBOTS 5


/* Static Funcitons */
static int gm_server_create_socket( uint16_t port );
static void *gm_server_run( void *ptr );
static int gm_server_read_from_client( gm_server_t * self, int filedes);
static void gm_server_parse_msg( gm_server_t * self, const char * msg, const int filedes );
static void gm_server_parse_field( const char * msg, float * field );
static void gm_server_parse_world( const char * msg, float * world );
static void gm_server_build_field_msg( const float * field, char * ret_msg );
static void gm_server_build_world_msg( const float * world, char * ret_msg );
    

/** Cria um novo objeto server_t.

 *
 * @param f_real_world FLAG que indica se o servidor que esta sendo
 * criado é um servidor no mundo real, ou seja, irá controlar robôs
 * reais ou um simulado.
 * 
 * @param port Porta que o servidor ficará ouvindo por conexões.
 *
 * @return Um objeto server_t recém criado.
 */
gm_server_t *
gm_server_new( uint16_t port )
{
    gm_server_t * self = NULL;

    self = XMALLOC(gm_server_t, 1);
    
    self->socketfd = gm_server_create_socket( port );

    if( listen( self->socketfd, 1 ) < 0 )
	xerror_fatal("server: Não consegui me transformar em um servidor.");

    /* Iniciando o conjunto de file descriptors */
    FD_ZERO( &(self->active_fd_set) );
    FD_SET( self->socketfd, &(self->active_fd_set) );

    /* Inicia ponteiro do monitor */
    self->monitor = NULL;

    /* Inicia ponteiro do controle */
    self->control = NULL;
	
    return self;
}

void
gm_server_add_control( gm_server_t * self, gm_control_t * control )
{
    self->control = control;
}

void
gm_server_add_monitor( gm_server_t * self, gm_monitor_t * monitor )
{
    self->monitor = monitor;
}


/** Destroi um servidor.
 *
 * @param *self Objeto server.
 */
void
gm_server_destroy( gm_server_t * self )
{
    int i = 0;
    void ** thread_return = NULL;

    /* Cancelando a thread */
    pthread_cancel( self->server_thread );
    pthread_join( self->server_thread,
		  thread_return );
    
    for (i = 0; i < FD_SETSIZE; ++i)
	if (FD_ISSET (i, &self->read_fd_set))
	{
	    close (i);
	}

    XFREE( self );
}


/** Cria um novo socket.
 *
 * @param port o número da porta que será ouvida.
 *
 * @return O número do file descriptor criado.
 */
static int
gm_server_create_socket( uint16_t port )
{
    int sock;
    struct sockaddr_in name;
     
    /* Create the socket. */
    sock = socket (PF_INET, SOCK_STREAM, 0);
    if (sock < 0)
	xerror_fatal("server: Não sonsegui criar um socket.");
     
    /* Give the socket a name. */
    name.sin_family = AF_INET;
    name.sin_port = htons (port);
    name.sin_addr.s_addr = htonl (INADDR_ANY);
    if (bind (sock, (struct sockaddr *) &name, sizeof (name)) < 0)
	xerror_fatal("server: Não consegui realizar bind "
		     "no socket recém criado.");
    
    return sock;
}

static void
gm_server_build_field_msg( const float * field,
			   char * ret_msg )
{
    int i = 0;
    char info[32];
    
    /* Primeiro, "limpamos" a mensagem */
    ret_msg[0] = '\0';

    for( i = 0 ; i < LENGTH_FIELD_VECTOR ; i++ )
    {
	gm_utils_float2str( info, field[i], 32);
	strcat( ret_msg, info );
	strcat( ret_msg, "|" );
    }
}

static void
gm_server_build_world_msg( const float * world,
			   char * ret_msg )
{
    int i = 0;
    char info[32];
    
    /* Primeiro, "limpamos" a mensagem */
    ret_msg[0] = '\0';

    for( i = 0 ; i < LENGTH_WORLD_VECTOR ; i++ )
    {
	gm_utils_float2str( info, world[i], 32);
	strcat( ret_msg, info );
	strcat( ret_msg, "|" );
    }
}

static void
gm_server_parse_world( const char * msg,
		       float * world )
{
    const char *ptr = msg;
    char buff[32];
    int n;
    int i = 0;
    while ( sscanf(ptr, "%31[^|]%n", buff, &n) == 1 )
    {
	sscanf( buff, "%f", &(world[i]));
	ptr += n;
	if ( *ptr != '|' )
	{
	    break;
	}
	++ptr;
	i++;
    }
}

static void
gm_server_parse_field( const char * msg,
		       float * field )
{
    const char *ptr = msg;
    char buff[32];
    int n;
    int i = 0;
    while ( sscanf(ptr, "%31[^|]%n", buff, &n) == 1 )
    {
	sscanf( buff, "%f", &(field[i]));
	ptr += n;
	if ( *ptr != '|' )
	{
	    break;
	}
	++ptr;
	i++;
    }
}

/** Passa a mensagem recebida pelo cliente.
 *
 * @param *self Objeto gm_server_t.
 *
 * @param *msg Mensagem a ser passada.
 */
static void
gm_server_parse_msg( gm_server_t * self, const char * msg, const int filedes )
{
    int info = 0;
    int iter = 0;
    float world[LENGTH_WORLD_VECTOR];
    float field[LENGTH_FIELD_VECTOR];

    info = gm_utils_vec_get_next_int( msg, &iter );
    
    switch( info )
    {
	/* Mensagem da Visão */
    case GM_MSG_VISION:
    {
      info = gm_utils_vec_get_next_int( msg, &iter );
	switch( info )
	{
	case 0:			/* Pacote de calibração */
	{
	    /* Traduz o vetor para um vetor de World */
	    gm_server_parse_world( &msg[4],
	    			   world );
	    
	    /* Repassa o vetor de inteiros para o monitor */
	    gm_monitor_set_world( self->monitor,
				  world );

	    break;
	}
	case 1:			/* Pacote de jogo */
	{	    
	    /* Traduz o vetor para um vetor Field */
	    gm_server_parse_field( &msg[4],
				   field );

	    /* Repassa o vetor de campo para o monitor */
	    gm_monitor_set_field( self->monitor,
				  field );

	    break;
	}
	}
	break;
    }
    /* Mensagem da IA */
    case GM_MSG_IA:
    {
	int nrobot = gm_utils_vec_get_next_int( msg, &iter );
	info = gm_utils_vec_get_next_int( msg, &iter );
	switch( info )
	{
	case GM_MSG_CMD_SAY_HELLO:
	{
	    xerror_info("O robô %d diz OLA.", nrobot);
	    break;
	}
	case GM_MSG_CMD_MOVE:
	{
	    float vx = gm_utils_vec_get_next_float( msg, &iter );
	    float vy = gm_utils_vec_get_next_float( msg, &iter );
	    float va = gm_utils_vec_get_next_float( msg, &iter );

	    gm_control_robot_set_vel( self->control,
				      nrobot,
				      vx,
				      vy,
				      va );
	    break;
	    
	}
	case GM_MSG_CMD_DRIBBLE_ENABLE:
	{
	    int enable = gm_utils_vec_get_next_int( msg, &iter );

	    gm_control_robot_dribble_enable( self->control,
					     nrobot,
					     enable );
	    break;
	}
	case GM_MSG_CMD_KICK:
	{
	    int nivel = gm_utils_vec_get_next_int( msg, &iter );

	    gm_control_robot_kick( self->control,
				   nrobot,
				   nivel );
	    break;
	}
	case GM_MSG_CMD_READ:
	{
	    char ret_msg[MAX_LENGTH_MSG];
   
	    gm_monitor_get_field( self->monitor,
				  field );

	    gm_server_build_field_msg( field,
				       ret_msg );
	    
	    write( filedes, ret_msg, MAX_LENGTH_MSG );

	    break;
	}
	case GM_MSG_CMD_READ_WORLD:
	{
	    char ret_msg[MAX_LENGTH_MSG];
   

	    gm_monitor_get_world( self->monitor,
				  world );

	    gm_server_build_world_msg( world,
				       ret_msg );
	    
	    write( filedes, ret_msg, MAX_LENGTH_MSG );

	    break;
	}

	}
	break;
    }
    default:
    {
	xerror_error("server: Início de pacote não esperado. Credo!");
    }
    }
}

/** Lê a mensagem do cliente conectado.
 *
 * @param *self Ponteiro para objeto gm_server_t.
 * 
 * @param filedes File descriptor do cliente.
 *
 * @return 0 em caso de leitura com sucesso e -1 caso o cliente queira
 * fechar a conexão.
 */
static int
gm_server_read_from_client( gm_server_t * self, int filedes)
{
    char buffer[MAX_LENGTH_MSG];
    int nbytes;
     
    nbytes = read (filedes, buffer, MAX_LENGTH_MSG);
    
    if (nbytes < 0)
    {
	xerror_error("server: Não foi possível ler a mensagem "
		     "enviada pelo cliente.");
	return 1;
    }
    else if (nbytes == 0)
	/* End-of-file. */
	return -1;
    else
    {
	/* Data read. */
	gm_server_parse_msg( self, buffer, filedes );
	return 0;
    }
}

/** Roda o servidor, espera por conexões e chama as funções
 * necessárias para interpretar as mensagens.
 *
 * @param *ptr Ponteiro para um objeto gm_server_t.
 */
static void
*gm_server_run( void *ptr )
{
    gm_server_t * self = (gm_server_t*) ptr;
    int i = 0;
    size_t size;
    struct sockaddr_in clientname;

    xerror_info("server: Servidor pronto e esperando.");
    while (1)
    {
	/* Block until input arrives on one or more active sockets. */
	self->read_fd_set = self->active_fd_set;
	if (select (FD_SETSIZE, &self->read_fd_set, NULL, NULL, NULL) < 0)
	    xerror_fatal("server: Tive problemas na chamada select");
    

	/* Service all the sockets with input pending. */
	for (i = 0; i < FD_SETSIZE; ++i)
	    if (FD_ISSET (i, &self->read_fd_set))
	    {
		if (i == self->socketfd)
		{
		    /* Connection request on original socket. */
		    int new;
		    size = sizeof (clientname);
		    new = accept (self->socketfd,
				  (struct sockaddr *) &clientname,
				  &size);
		    if (new < 0)
			xerror_fatal("server: Problemas em aceitar a conexão"
				     " requisitada");
		    xerror_info ( "server: Conexão do host %s, porta %d.",
			     inet_ntoa (clientname.sin_addr),
			     ntohs (clientname.sin_port));
		    FD_SET (new, &(self->active_fd_set));
		}
		else
		{
		    /* Data arriving on an already-connected socket. */
		    if (gm_server_read_from_client( self, i) < 0)
		    {
			close (i);
			FD_CLR (i, &(self->active_fd_set));
		    }
		}
	    }
    }
    
}

/** Inicia o servidor.
 *
 * @param *self Ponteiro para objeto gm_server_t.
 */
void
gm_server_init( gm_server_t * self )
{
    int iret1 = 0;

    iret1 = pthread_create( &(self->server_thread),
    			    NULL,
    			    gm_server_run,
    			    (void*) self );
}
